#include "control_base/WMJRobotControl.h"
#include <bits/stdint-uintn.h>

namespace wmj
{
    /**
     * @brief WMJRobotControl构造函数
     */
    WMJRobotControl::WMJRobotControl() : lastpose{0.f, 0.f}, poseInit{false}, circlecnt{0}
    {
        cv::FileStorage fs(CONTROL_CFG, cv::FileStorage::READ);
        fs["port_type"] >> m_port_type;
        fs["fifteen_shoot_speed"] >> m_fifteen_shoot_speed;
        fs["eighteen_shoot_speed"] >> m_eighteen_shoot_speed;
        fs["thirty_shoot_speed"] >> m_thirty_shoot_speed;
        fs.release();

        switch (m_port_type)
        {
        case 0:
            //this->canBus = std::make_shared<wmj::SerialPort>();
            //this->gyroCanBus = std::make_shared<wmj::SerialPort>();
            
            break;
        case 1:
            
            this->canBus = std::make_shared<wmj::CanPort>("can0");
            
            // this->canBus_spare = std::make_shared<wmj::CanPort>("can1");
            // if ((!this->canBus->canUseThisPort) && (!this->canBus_spare->canUseThisPort))
            // {
            //     std::cout << "[WMJERROR]Code=01 U2CAN can't open" << std::endl;
            //     std::cout << "init error" << std::endl;
            //     
            //         exit(0);
            // }
            // this->gyroCanBus = std::make_shared<wmj::CanPort>("can0");
            break;
        }
        this->LastState = STATE_ARMOR;
    }

    /**
     * @brief 切换基坐标系：Mode = false为底盘系，Mode = true为地面系
     *
     * @param Mode
     */
    void WMJRobotControl::SwitchBaseCoor(bool Mode)
    {
        _BaseCoor = (uint8_t)Mode * 0x40;
    }

    /*
     * @brief 向主控板发送相关信息
     *
     * @param armor
     * @param capture
     * @param distance
     * @return true
     * @return false
     */
    bool WMJRobotControl::sendInfo(uint8_t armor, uint8_t exposure_state, bool capture, float distance)
    {
        MCMPack control_data{};
        Buffer data_to_send{};

        // exposure_state 0x01, capture true正在自动曝光
        // exposure_state 0x02, capture true完成自动曝光
        switch (armor)
        {
        case 0x00:
            control_data.msg.mctl_base = 0x00 | 0x04 * capture;
            control_data.msg.mctl_if_end_exposure = exposure_state * capture;
            break;
        case 0x01:
            control_data.msg.mctl_base = 0x01 | 0x04 * capture;
            control_data.msg.mctl_if_end_exposure = exposure_state * capture;
            break;
        case 0x02:
            control_data.msg.mctl_base = 0x03 | 0x04 * capture;
            control_data.msg.mctl_if_end_exposure = exposure_state * capture;
            break;
        default:
            control_data.msg.mctl_base = 0x00;
            control_data.msg.mctl_if_end_exposure = exposure_state * capture;
        }
        control_data.msg.mctl_distance_low = (uint8_t)((uint16_t)distance);
        control_data.msg.mctl_distance_high = (uint8_t)((uint16_t)distance >> 8);
        control_data.msg.placehoder1 = 0;
        control_data.msg.placehoder2 = 0;
        control_data.msg.placehoder3 = 0;
        control_data.msg.placehoder4 = 0;

        data_to_send.push_back((uint8_t)MainControl);
        for (auto c : control_data.data)
            data_to_send.push_back(c);

        // sendFrame(data_to_send);
        // canBus->sendFrame(data_to_send);

        sendFrame(data_to_send);

        return true;
    }

    //////////////////////// 按键通讯协议 ////////////////////////
    /**
     * data[1]  0   S1_low      数据1~3分别代表左拨杆高低中
     * data[1]  1   S1_high
     * data[1]  2   S2_low      数据1~3分别代表右拨杆高低中
     * data[1]  3   S2_high
     * data[1]  4   mouse_left  自动击发
     * data[1]  5   mouse_right 自瞄模式
     *
     * data[6]  0   W           前进
     * data[6]  1   S           后退
     * data[6]  2   A           左移
     * data[6]  3   D           右移
     * data[6]  4   shift       加速
     * data[6]  5   ctrl        减速
     * data[6]  6   Q           逆向切换视觉模式
     * data[6]  7   E           正向切换视觉模式
     *
     * data[7]  0   R           关弹仓；Shift+R开弹仓
     * data[7]  1   F           应急打符按键，按住为打符模式
     * data[7]  2   G           应急复位按键，视觉模式重置为armor
     * data[7]  3   Z           正向更改曝光等级
     * data[7]  4   X           自动击发开关
     * data[7]  5   C
     * data[7]  6   V           固定底盘
     * data[7]  7   B           开启陀螺
     */
    ////////////////////////////////////////////////////////////

    /**
     * @brief 获取当前按键信息
     *
     * @return KeyboardInfo
     */
    KeyboardInfo WMJRobotControl::GetKeyboardInfo()
    {
        Buffer data_from_read;

        data_from_read = readFrame((int)MainControl);

        // bool aim_a, aim_r0, aim_r1, Rins, Rdes;
        wmj::KeyboardInfo Info;
        if (data_from_read.size() > 0)
        {
            /*
                视觉模式顺序
                ARMOR   普通自瞄
                TOP     反陀螺
                RUNE    打符
                DARK    打哨兵
            */

            // 自瞄模式
            // data1 0x05 左上右上
            // data1 0x09 左上右下
            // data1 0x20 右键
            // data1 0x30 左右键
            // 视觉模式正向切换 E
            // data6 0x80 E
            // 视觉模式逆向切换 Q
            // data6 0x40 Q
            // 应急复位按键，视觉模式重置为armor G
            // data7 0x04 G
            // 应急打符按键，按住为打符模式
            // data7 0x02 F
            // 正向曝光等级切换 Z
            // data7 0x08 Z
            // 自动击发开关 X
            // data7 0x10 X

            // 更改曝光值 Z 右键不能按下
            // data7 0x08 Z
            bool flag_z = !((uint8_t)data_from_read[1] & 0x20) && ((uint8_t)data_from_read[7] & 0x08);
            checkCLickAndSetStatus(flag_z, m_click.click_z, Info.change_exposure);

            // 自动击发开关 X
            // data7 0x10 X
            bool flag_x = (uint8_t)data_from_read[7] & 0x010;
            checkCLickAndSetStatus(flag_x, m_click.click_x, Info.auto_shoot_switch);

            // 视觉模式正向切换 E
            // Q、G、右键不能按下
            // data6 0x80 E
            bool flag_e = ((uint8_t)data_from_read[6] & 0x80) &&
                         !((uint8_t)data_from_read[6] & 0x40) &&
                         !((uint8_t)data_from_read[7] & 0x04) &&
                         !((uint8_t)data_from_read[1] & 0x20);
            checkCLickAndSetStatus(flag_e, m_click.click_e, Info.aim_next);

            // 视觉模式逆向切换 Q
            // E、G、右键不能按下
            // data6 0x40 Q
            bool flag_q = ((uint8_t)data_from_read[6] & 0x40) &&
                          !((uint8_t)data_from_read[6] & 0x80) &&
                          !((uint8_t)data_from_read[7] & 0x04) &&
                          !((uint8_t)data_from_read[1] & 0x20);
            checkCLickAndSetStatus(flag_q, m_click.click_q, Info.aim_last);

            // 视觉模式重置为默认 G
            // E、Q、右键不能按下
            // data7 0x04 G
            bool flag_g = ((uint8_t)data_from_read[7] & 0x04) &&
                         !((uint8_t)data_from_read[6] & 0x80) &&
                         !((uint8_t)data_from_read[6] & 0x40) &&
                         !((uint8_t)data_from_read[1] & 0x20);
            checkCLickAndSetStatus(flag_g, m_click.click_g, Info.aim_reset);

            // 键鼠模式按右键
            // data1 0x20 右键
            if ((uint8_t)data_from_read[1] & 0x20)
            {
                // 应急打符按键 按住F
                // data7 0x02 F
                if ((uint8_t)data_from_read[7] & 0x02)
                {
                    Info.aim_rune_em = true;
                }
                else
                {
                    Info.aim_rune_em = false;
                }

                // 击发
                // data1 0x10 左键
                if ((uint8_t)data_from_read[1] & 0x10)
                {
                    Info.enable_shoot = true;
                }
                else
                {
                    Info.enable_shoot = false;
                }
                Info.use_number_detect = true;
                Info.aim_armor = true;
            }
            else
            {
                Info.aim_armor = false;
                Info.enable_shoot = false;
                Info.use_number_detect = true;
            }

            // 微调位姿
            bool flag_w = (uint8_t)data_from_read[6] & 0x01;
            checkCLickAndSetStatus(flag_w, m_click. click_w, Info.up);
            bool flag_s = (uint8_t)data_from_read[6] & 0x02;
            checkCLickAndSetStatus(flag_s, m_click. click_w, Info.down);
            bool flag_a = (uint8_t)data_from_read[6] & 0x04;
            checkCLickAndSetStatus(flag_a, m_click. click_w, Info.left);
            bool flag_d = (uint8_t)data_from_read[6] & 0x08;
            checkCLickAndSetStatus(flag_d, m_click. click_w, Info.right);
        }
        // std::cout << "[info]"<< ((uint8_t)data_from_read[1] & 0x10) << " "
        //                      << ((uint8_t)data_from_read[1] & 0x20) << " "
        //                      << ((uint8_t)data_from_read[7] & 0x02) << " "
        //                      << ((uint8_t)data_from_read[1]) << " "
        //                      << ((uint8_t)data_from_read[7]) << std::endl
        //          << "[info]aim_armor:\t" << Info.aim_armor << std::endl
        //          << "[info]enable_shoot:\t" << Info.enable_shoot << std::endl
        //          << "[info]aim_last:\t" << Info.aim_last << std::endl
        //          << "[info]aim_next\t" << Info.aim_next << std::endl
        //          << "[info]change_exposure:\t" << Info.change_exposure << std::endl
        //          << "[info]use_number_detect:\t" << Info.use_number_detect << std::endl
        return Info;
    }

    /**
     *@brief: 获取发送的命令
     *
     *@return: ROBOSTATE 发送的命令
     */
    ROBO_STATE WMJRobotControl::GetRobotStatus()
    {
        KeyboardInfo robotStatus;
        robotStatus = this->GetKeyboardInfo();
        bool rune_em = false;

        if (robotStatus.auto_shoot_switch) // 自动击发
            m_auto_shoot = !m_auto_shoot;  // 切换自动击发状态

        if (robotStatus.change_exposure) // 更改曝光值 打符模式下无效
        {
            if (m_cur_robo_state == STATE_DARK)
            {
                if (m_dark_camera_exposure == EXPOSURE_HIGH)
                    this->m_dark_camera_exposure = EXPOSURE_LOW;
                else
                    this->m_dark_camera_exposure = CAMERA_EXPOSURE(m_dark_camera_exposure + 1);
            }
            else if (m_cur_robo_state == STATE_ARMOR)
            {
                if (m_armor_camera_exposure == EXPOSURE_HIGH)
                    this->m_armor_camera_exposure = EXPOSURE_LOW;
                else
                    this->m_armor_camera_exposure = CAMERA_EXPOSURE(m_armor_camera_exposure + 1);
            }
        }

        if (robotStatus.aim_armor) // 启用自瞄
        {
            // 应急状态打符模式
            if (robotStatus.aim_rune_em)
                rune_em = true;

            if (robotStatus.enable_shoot) // 击发
            {
                this->m_enable_shoot = true;
                std::cout << "ARMOR SHOOTING\n";
            }
            else
            {
                this->m_enable_shoot = false;
            }
            this->m_aim_armor = true;
        }
        else
        {
            this->m_aim_armor = false;
        }

        if (robotStatus.aim_next)
        {
            if (m_cur_robo_state == STATE_DARK)
                this->m_cur_robo_state = STATE_ARMOR;
            else
                this->m_cur_robo_state = ROBO_STATE(m_cur_robo_state + 1);
        }

        if (robotStatus.aim_last)
        {
            if (m_cur_robo_state == STATE_ARMOR)
                this->m_cur_robo_state = STATE_DARK;
            else
                this->m_cur_robo_state = ROBO_STATE(m_cur_robo_state - 1);
        }

        if (robotStatus.aim_reset)
        {
            this->m_cur_robo_state = STATE_ARMOR;
            this->m_armor_camera_exposure = EXPOSURE_MIDDLE;
            this->m_dark_camera_exposure = EXPOSURE_HIGH;
            this->m_auto_shoot = true;
            this->m_adjust_pose = GimbalPose(0, 0);
        }

        LastState = m_cur_robo_state;
        if (rune_em)
        {
            std::cout << "[ROBO_STATE]\t" << wmj::ROBO_STATE::STATE_RUNE << std::endl;

            if (robotStatus.up)
                this->m_adjust_pose.pitch -= 0.002;
            if (robotStatus.down)
                this->m_adjust_pose.pitch += 0.002;
            if (robotStatus.left)
                this->m_adjust_pose.yaw += 0.002;
            if (robotStatus.right)
                this->m_adjust_pose.yaw -= 0.002;

            std::cout << "adjust_pose: " << m_adjust_pose << std::endl;

            return wmj::ROBO_STATE::STATE_RUNE;
        }
        else
        {
            std::cout << "[ROBO_STATE]\t" << m_cur_robo_state << std::endl;
            return m_cur_robo_state;
        }
    }

    /**
     * @brief 获取更改后的相机曝光，需在GetRobotStatus后运行
     *
     * @return string 相机曝光等级
     */
    std::string WMJRobotControl::GetCameraExposure()
    {
        CAMERA_EXPOSURE exposure;
        if (m_cur_robo_state == wmj::ROBO_STATE::STATE_DARK)
            exposure = m_dark_camera_exposure;
        else
            exposure = m_armor_camera_exposure;

        switch (exposure)
        {
        case EXPOSURE_LOW:
            return "low";
        case EXPOSURE_MIDDLE:
            return "middle";
        case EXPOSURE_HIGH:
            return "high";
        default:
            break;
        }

        return "middle";
    }
    void WMJRobotControl::sendFrame(Buffer &data_to_send)
    {
        if (canBus->canUseThisPort)
        {
            canBus->sendFrame(data_to_send);
        }
        // else if (canBus_spare->canUseThisPort)
        // {
        //     canBus_spare->sendFrame(data_to_send);
        // }
        else
        {
            std::cout << "[WMJERROR]Code=02 U2CAN can't open" << std::endl;
            
                exit(0);
        }
    }
    Buffer WMJRobotControl::readFrame(int flag)
    {
        Buffer data;
        if (canBus->canUseThisPort)
        {
            data = canBus->readFrame(flag);
        }
        // else if (canBus_spare->canUseThisPort)
        // {
        //     data = canBus_spare->readFrame(flag);
        // }
        else
        {
            std::cout << "[WMJERROR]Code=03 U2CAN can't open" << std::endl;
                exit(0);
        }
        return data;
    }

    /**
     * @brief 获取当前微调位姿补偿，需要在需在GetRobotStatus后运行
     */
    GimbalPose WMJRobotControl::GetAdjustPose()
    {
        return this->m_adjust_pose;
    }

    /**
     * @brief 根据按键点击逻辑切换状态量。由于该逻辑复用过多，所以单独写成一个函数
     */
    void WMJRobotControl::checkCLickAndSetStatus(bool flag, bool &click, bool &status)
    {
        if (flag)
        {
            // 如果按键当前按下且点击标志位为否，置真
            if (!click)
                click = true;
            status = false;
        }
        else
        {
            // 如果按键当前松开且点击标志位为真，即认为之前按键是按下状态，认为完成了一次点击
            // 将状态量置真，点击标志位复原
            if (click)
            {
                click = false;
                status = true;
            }
            else
            {
                status = false;
            }
        }
    }
}
